#ifndef __MSG_CONVERT_H__
#define __MSG_CONVERT_H__

#include <geometry_msgs/Pose.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 

#include <iostream>
#include <vector>
using namespace std;

#include "pose_2d.h"
#include "pixel_2d.h"

namespace msg_convert{
    // tf2::Quaternion -> Euler
    std::tuple<double, double, double> Quaternion2Euler(tf2::Quaternion quaternion);
    // geometry_msgs::Quaternion -> Euler
    std::tuple<double, double, double> GeometryQuaternion2Euler(geometry_msgs::Quaternion orientation);
    // Euler -> tf2::Quaternion
    tf2::Quaternion Euler2Quaternion(double roll, double pitch, double yaw);
    // Euler -> tf2::Quaternion
    geometry_msgs::Quaternion Euler2GeometryQuaternion(double roll, double pitch, double yaw);

    // Pose2D -> geometry_msgs::Pose
    geometry_msgs::Pose Pose2D2Pose(Pose2D pose_2d);
    // geometry_msgs::Pose -> Pose2D
    Pose2D Pose2Pose2D(geometry_msgs::Pose pose);

    // Pose2D -> geometry_msgs::TransformStamped
    geometry_msgs::TransformStamped Pose2D2TransformStamped(Pose2D pose_2d);

    // tf2::Transform -> geometry_msgs::TransformStamped
    geometry_msgs::TransformStamped Transform2Geometry(tf2::Transform transform_tf);
    // geometry_msgs::TransformStamped -> tf2::Transform
    tf2::Transform Geometry2Transform(geometry_msgs::TransformStamped transform_stamped);

} // namespace MapConvert

#endif